#include <iostream>
#include <math.h>
#include "determine_drive_command.h"
#include "blobdetector.h"

#define DUMP(x) cout << #x << ": " << x << endl
#define PIXEL_PER_M 190
#define ANGLE_THRESH M_PI/2
#define DIST_THRESH 85
#define MAX_RESPONSE 10
#define KV (MAX_RESPONSE/(DIST_THRESH*5)) // set v gain so is max when 3*dist_thresh
#define KW (MAX_RESPONSE/M_PI) // Set turn gain so that is max when angle error = pi

using namespace std;
using namespace cv;

DriveCommand determineDriveCommand2(RobotOrientation robot, Point fruit)
{
  // Displacement error
  int xErr = fruit.x - robot.point.x;
  int yErr = fruit.y - robot.point.y;
  double thetaRef; // Desired theta
  double thetaErr; // Angle error
  DriveCommand output;

  if( !xErr ) // check for divide by zero
    {
    if (!yErr) 
      { // this shouldn't ever happen since robot can't be exactly where fruit is
      output.dir = DIR_STOP, output.speed = 0;
      return output;
      }
    if (yErr > 0)
      thetaRef = M_PI/2;
    else
      thetaRef = -M_PI/2;
    }
  else 
    thetaRef = atan2(yErr,xErr) ; // Get angle of goal relative to robot

  thetaErr = thetaRef - (robot.angle*M_PI/180);
  if( thetaErr != 0 ) thetaErr = atan2( sin(thetaErr), cos(thetaErr) );

  if ( abs(thetaErr) < ANGLE_THRESH ) // angle error is within threshold
    { // Set forward velocity
    double distError = sqrt(pow((double)xErr, 2) + pow((double)yErr, 2));
    if( distError < DIST_THRESH ) 
      {
      cout << "CLOSE ENOUGH!" << endl;
      output.dir = DIR_STOP, output.speed = 0;
      return output; // at the goal!
      }
    output.speed = (int) KV * distError;
    output.dir = DIR_FORWARD;
    }
  else 
    { // Set turn velocity
    int w = (int) KW * thetaErr;
    if( w < 0 )
      output.dir = DIR_ROT_LEFT;
    else
      output.dir = DIR_ROT_RIGHT;
    output.speed = abs(w);
    }

  if(output.speed > MAX_RESPONSE) {
    output.speed = MAX_RESPONSE; // Saturate speed at 10
    cout << "Response too high!" << endl;
  }
  else if(!output.speed) // If getting a zero speed
    output.dir = DIR_STOP; // Stop the rovio
    
  return output;
}

DriveDirection determineDriveCommand(RobotOrientation robot, Point fruit,
	double threshold)
{
	double fruitAngle;

	if(robot.point.x == fruit.x){
		if(robot.point.y > fruit.y){
			fruitAngle = 0;
		}else{
			fruitAngle = 180;
		}
	}else{
	  fruitAngle = atan2( (double)(fruit.y - robot.point.y), (double)(fruit.x - robot.point.x))/M_PI*180;
		if(fruitAngle<0)
			fruitAngle += 360;
	}
	
	DUMP(fruitAngle);
	
	DriveDirection command;
	
	if( euclidean(robot.point,fruit) < 85 ) // check if robot is close enough
	    command = DIR_STOP;
	else if (fabs(robot.angle - fruitAngle) < threshold) 
	{
	  cout << "Forward" << endl;
	  command = DIR_FORWARD;
	} else if (robot.angle < fruitAngle) {
	  cout << "Right" << endl;
	  command = DIR_ROT_RIGHT;
	} else {
	  cout << "LEFT" << endl;
		command = DIR_ROT_LEFT;
	}
	
	return command;
}

DriveDirection determineDriveCommand3(RobotOrientation robot, Point fruit) {
	double fruitAngle;

	if(robot.point.x == fruit.x){
		if(robot.point.y > fruit.y){
			fruitAngle = 0;
		}else{
			fruitAngle = 180;
		}
	}else{
	  fruitAngle = atan2( (double)(fruit.y - robot.point.y), (double)(fruit.x - robot.point.x))/M_PI*180;
		if(fruitAngle<0)
			fruitAngle += 360;
	}
	
	DUMP(fruitAngle);
	
	double difference = robot.angle - fruitAngle;
	
	if (difference < 0) {
		difference += 360;
	}
	
	DUMP(difference);
	
	if (difference < 22.5) {
		return DIR_FORWARD;
	} else if (difference < 67.5) {
		return DIR_DIAG_FOR_LEFT;
	} else if (difference < 112.5) {
		return DIR_LEFT;
	} else if (difference < 157.5) {
		return DIR_DIAG_BACK_LEFT;
	} else if (difference < 202.5) {
		return DIR_BACKWARD;
	} else if (difference < 247.5) {
		return DIR_DIAG_BACK_RIGHT;
	} else if (difference < 292.5) {
		return DIR_RIGHT;
	} else if (difference < 337.5) {
		return DIR_DIAG_FOR_RIGHT;
	} else {
		return DIR_FORWARD;
	}
}